#!/usr/bin/env python
import rospy
import math
from nav_msgs.msg import Path

new_path_pub = rospy.Publisher('/tianracer/ros1_talk', Path, queue_size=5)
ds = 0.5

def callback(data):
    pub_msg = Path()
    pub_msg.header = data.header
    pub_msg.poses = data.poses
    for pose in pub_msg.poses:
        theda = pose.pose.orientation.z
        pose.pose.position.x += ds*math.sin(theda)
        pose.pose.position.y += ds*math.cos(theda)

    new_path_pub.publish(pub_msg)

    # print(1)
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.poses)

    
def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.

    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("/move_base/GlobalPlanner/plan", Path, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    listener()